#include "common.h"

unsigned char brake_state          = 0;
unsigned char uperMonitor          = 0;
unsigned char master_comm_err      = 0;
unsigned char water_press_err_cnt  = 0;
unsigned char qbh_err_cnt          = 0;
unsigned char brake_cnt            = 0;
unsigned int  master_comm_err_cnt  = 0;
ULONG         power_comm_timestamp = 0;
ULONG         pump_comm_timestamp[4];

volatile unsigned char pd1_check_enable = 0;
unsigned char          pd1_check_start  = 0;
TX_TIMER               cycle_check_timer;

TX_QUEUE queue_emit_success;
ULONG    queue_emit_success_buf;

TX_THREAD     pd1_check_thread_handle;
unsigned char pd1_check_thread_stack[1024];

TX_THREAD     pd_success_check_thread_handle;
unsigned char pd_success_check_thread_stack[1024];

TX_THREAD     pd2_check_thread_handle;
unsigned char pd2_check_thread_stack[1024];

TX_THREAD     pump_pd_check_thread_handle;
unsigned char pump_pd_check_thread_stack[1024];

unsigned char pd1_underlow_count;
unsigned char pd2_underlow_count;
unsigned char pd3_underlow_count;

unsigned char t1_error_cnt;
unsigned char t2_error_cnt;
unsigned char t3_error_cnt;
unsigned char t4_error_cnt;

unsigned char envi_temp_err_cnt;
unsigned char envi_humidity_err_cnt;

unsigned char modul1_State_Mask;
unsigned char modul2_State_Mask;
unsigned char modul3_State_Mask;
unsigned char modul4_State_Mask;

unsigned char err_dispose_finish = 0;

void init_check_var(void)
{
    memory_set(&pd1_underlow_count, 0, sizeof(pd1_underlow_count));
    memory_set(&pd2_underlow_count, 0, sizeof(pd2_underlow_count));
    memory_set(&t1_error_cnt, 0, sizeof(t1_error_cnt));
    memory_set(&t1_error_cnt, 0, sizeof(t1_error_cnt));
    memory_set(&t1_error_cnt, 0, sizeof(t1_error_cnt));
    memory_set(&t1_error_cnt, 0, sizeof(t1_error_cnt));

    memory_set(&envi_temp_err_cnt, 0, sizeof(envi_temp_err_cnt));
    memory_set(&envi_humidity_err_cnt, 0, sizeof(envi_humidity_err_cnt));

    memory_set((unsigned char *)pump_comm_timestamp, 0, sizeof(pump_comm_timestamp));

    brake_cnt            = 0;
    brake_state          = 0;
    master_comm_err      = 0;
    water_press_err_cnt  = 0;
    qbh_err_cnt          = 0;
    master_comm_err_cnt  = 0;
    power_comm_timestamp = 0;
    pd1_check_enable     = 0;
    pd1_check_start      = 0;

    modul1_State_Mask = 0;
    modul2_State_Mask = 0;
    modul3_State_Mask = 0;
    modul4_State_Mask = 0;

    err_dispose_finish = 0;

    tx_timer_create(&cycle_check_timer, 0, cycle_check_timer_callback, 0, 10, 10, TX_NO_ACTIVATE);

    tx_queue_create(&queue_emit_success, NULL, 1, &queue_emit_success_buf, sizeof(queue_emit_success_buf));
}

void clear_check_cnt(void)
{
    memory_set(&t1_error_cnt, 0, sizeof(t1_error_cnt));
    memory_set(&t2_error_cnt, 0, sizeof(t2_error_cnt));
    memory_set(&t3_error_cnt, 0, sizeof(t3_error_cnt));
    memory_set(&t4_error_cnt, 0, sizeof(t4_error_cnt));
    memory_set(&envi_temp_err_cnt, 0, sizeof(envi_temp_err_cnt));
    memory_set(&envi_humidity_err_cnt, 0, sizeof(envi_humidity_err_cnt));
    memory_set(&pd1_underlow_count, 0, sizeof(pd1_underlow_count));
    memory_set(&pd2_underlow_count, 0, sizeof(pd2_underlow_count));
    brake_cnt            = 0;
    brake_state          = 0;
    master_comm_err      = 0;
    water_press_err_cnt  = 0;
    qbh_err_cnt          = 0;
    master_comm_err_cnt  = 0;
    pd1_check_enable     = 0;
    pd1_check_start      = 0;
    err_dispose_finish   = 0;
    power_comm_timestamp = tx_time_get();
    output_seq_err_cnt   = 0;

    modul1_State_Mask = 0;
    modul2_State_Mask = 0;
    modul3_State_Mask = 0;
    modul4_State_Mask = 0;

    for (unsigned int i = 0; i < 4; i++) {
        pump_comm_timestamp[i] = tx_time_get();
    }
}

void Operating_Parameter_Check(void)
{
    ntc_check();
    environment_check();
}

void cycle_check_timer_callback(ULONG arg)
{
    feed_iwdg();
    get_sensor_data();
    Operating_Parameter_Check();
    GPIO_Check();

    if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_2.part.mo) {
        if (operate_parm.power_operate_parm.modul1_C_Out > adaptor_parm.part.power_output_adaptor_parm.level1PreCur_up_limit) {
            operate_parm.power_operate_parm.modul1_State |= (1 << 0);
            Laser_Health_Refresh();
        }
        if (operate_parm.power_operate_parm.modul2_C_Out > adaptor_parm.part.power_output_adaptor_parm.level2PreCur_up_limit) {
            operate_parm.power_operate_parm.modul2_State |= (1 << 0);
            Laser_Health_Refresh();
        }
    } else {
        operate_parm.power_operate_parm.modul1_State &= ~(1 << 0);
        operate_parm.power_operate_parm.modul2_State &= ~(1 << 0);
    }

    if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_2.part.fa) {
        unsigned short current;
        current = (operate_parm.power_operate_parm.modul3_C_Out_H << 16) | operate_parm.power_operate_parm.modul3_C_Out_L;
        if (current > adaptor_parm.part.power_output_adaptor_parm.fa_up_limit) {
            operate_parm.power_operate_parm.modul3_State |= (1 << 0);
            Laser_Health_Refresh();
        }
        current = (operate_parm.power_operate_parm.modul4_C_Out_H << 16) | operate_parm.power_operate_parm.modul4_C_Out_L;
        if (current > adaptor_parm.part.power_output_adaptor_parm.fa_up_limit) {
            operate_parm.power_operate_parm.modul4_State |= (1 << 0);
            Laser_Health_Refresh();
        }
    } else {
        operate_parm.power_operate_parm.modul3_State &= ~(1 << 0);
        operate_parm.power_operate_parm.modul4_State &= ~(1 << 0);
    }

    master_comm_err_cnt++;
    if (1000 <= master_comm_err_cnt) {
        master_comm_err_cnt = 1000;
        master_comm_err     = 1;
        uperMonitor         = 0;
        Laser_Health_Refresh();
    }
}

void environment_check(void)
{
    /*ET_Check*/
    if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_2.part.env_temp) {
        if ((operate_parm.laser_operate_data.ET_Value > adaptor_parm.part.laser_module_adaptor_parm.env_temp_up_limit) ||
            (operate_parm.laser_operate_data.ET_Value < adaptor_parm.part.laser_module_adaptor_parm.env_temp_low_limit)) {
            envi_temp_err_cnt++;
            if (10 <= envi_temp_err_cnt) {
                envi_temp_err_cnt                                        = 10;
                operate_parm.laser_operate_data.fault_code2.bit.envi_err = 1;
                Laser_Health_Refresh();
            }
        } else {
            envi_temp_err_cnt = 0;
        }

        if ((operate_parm.laser_operate_data.ET_Value < (adaptor_parm.part.laser_module_adaptor_parm.env_temp_up_limit - 3)) &&
            (operate_parm.laser_operate_data.ET_Value > (adaptor_parm.part.laser_module_adaptor_parm.env_temp_low_limit + 3)))  // 滞回
        {
            envi_temp_err_cnt                                        = 0;
            operate_parm.laser_operate_data.fault_code2.bit.envi_err = 0;
        }
    } else {
        envi_temp_err_cnt                                        = 0;
        operate_parm.laser_operate_data.fault_code2.bit.envi_err = 0;
    }

    /*EH_Check*/
    if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_2.part.env_hum) {
        if (operate_parm.laser_operate_data.EH_Value > adaptor_parm.part.laser_module_adaptor_parm.env_hum_up_limit) {
            envi_humidity_err_cnt++;
            if (10 <= envi_humidity_err_cnt) {
                envi_humidity_err_cnt                                        = 10;
                operate_parm.laser_operate_data.fault_code2.bit.humidity_err = 1;
                Laser_Health_Refresh();
            }
        } else {
            envi_humidity_err_cnt = 0;
        }

        if (operate_parm.laser_operate_data.EH_Value < (adaptor_parm.part.laser_module_adaptor_parm.env_hum_up_limit - 2))  // 滞回
        {
            envi_humidity_err_cnt                                        = 0;
            operate_parm.laser_operate_data.fault_code2.bit.humidity_err = 0;
        }
    } else {
        envi_humidity_err_cnt                                        = 0;
        operate_parm.laser_operate_data.fault_code2.bit.humidity_err = 0;
    }
}

void ntc_check(void)
{
    if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_1.part.t1) {
        if ((operate_parm.laser_operate_data.T1_Value <= adaptor_parm.part.laser_module_adaptor_parm.t1_low_limit) ||
            (operate_parm.laser_operate_data.T1_Value >= adaptor_parm.part.laser_module_adaptor_parm.t1_up_limit)) {
            t1_error_cnt++;
            if (10 <= t1_error_cnt) {
                t1_error_cnt                                       = 10;
                operate_parm.laser_operate_data.fault_code1.bit.t1 = 1;
                Laser_Health_Refresh();
            }
        } else {
            t1_error_cnt = 0;
        }

        if ((operate_parm.laser_operate_data.T1_Value < (adaptor_parm.part.laser_module_adaptor_parm.t1_up_limit - 3)) &&
            (operate_parm.laser_operate_data.T1_Value > (adaptor_parm.part.laser_module_adaptor_parm.t1_low_limit + 3))) {
            operate_parm.laser_operate_data.fault_code1.bit.t1 = 0;
        }
    }
    //////t2 check
    if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_1.part.t2) {
        if ((operate_parm.laser_operate_data.T2_Value <= adaptor_parm.part.laser_module_adaptor_parm.t2_low_limit) ||
            (operate_parm.laser_operate_data.T2_Value >= adaptor_parm.part.laser_module_adaptor_parm.t2_up_limit)) {
            t2_error_cnt++;
            if (10 <= t2_error_cnt) {
                t2_error_cnt                                       = 10;
                operate_parm.laser_operate_data.fault_code1.bit.t2 = 1;
                Laser_Health_Refresh();
            }
        } else {
            t2_error_cnt = 0;
        }

        if ((operate_parm.laser_operate_data.T2_Value < (adaptor_parm.part.laser_module_adaptor_parm.t2_up_limit - 3)) &&
            (operate_parm.laser_operate_data.T2_Value > (adaptor_parm.part.laser_module_adaptor_parm.t2_low_limit + 3))) {
            operate_parm.laser_operate_data.fault_code1.bit.t2 = 0;
        }
    } else {
        t2_error_cnt                                       = 0;
        operate_parm.laser_operate_data.fault_code1.bit.t2 = 0;
    }

    // t3 check

    if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_1.part.t3) {
        if ((operate_parm.laser_operate_data.T3_Value <= adaptor_parm.part.laser_module_adaptor_parm.t3_low_limit) ||
            (operate_parm.laser_operate_data.T3_Value >= adaptor_parm.part.laser_module_adaptor_parm.t3_up_limit)) {
            t3_error_cnt++;
            if (10 <= t3_error_cnt) {
                t3_error_cnt                                       = 10;
                operate_parm.laser_operate_data.fault_code1.bit.t3 = 1;
                Laser_Health_Refresh();
            }
        } else {
            t3_error_cnt = 0;
        }

        if ((operate_parm.laser_operate_data.T3_Value < (adaptor_parm.part.laser_module_adaptor_parm.t3_up_limit - 3)) &&
            (operate_parm.laser_operate_data.T3_Value > (adaptor_parm.part.laser_module_adaptor_parm.t3_low_limit + 3))) {
            operate_parm.laser_operate_data.fault_code1.bit.t3 = 0;
        }
    } else {
        t3_error_cnt                                       = 0;
        operate_parm.laser_operate_data.fault_code1.bit.t3 = 0;
    }

    // t4 check
    if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_1.part.t4) {
        if ((operate_parm.laser_operate_data.T4_Value <= adaptor_parm.part.laser_module_adaptor_parm.t4_low_limit) ||
            (operate_parm.laser_operate_data.T4_Value >= adaptor_parm.part.laser_module_adaptor_parm.t4_up_limit)) {
            t4_error_cnt++;
            if (10 <= t4_error_cnt) {
                t4_error_cnt                                       = 10;
                operate_parm.laser_operate_data.fault_code1.bit.t4 = 1;
                Laser_Health_Refresh();
            }
        } else {
            t4_error_cnt = 0;
        }

        if ((operate_parm.laser_operate_data.T4_Value < (adaptor_parm.part.laser_module_adaptor_parm.t4_up_limit - 3)) &&
            (operate_parm.laser_operate_data.T4_Value > (adaptor_parm.part.laser_module_adaptor_parm.t4_low_limit + 3))) {
            operate_parm.laser_operate_data.fault_code1.bit.t4 = 0;
        }
    } else {
        t4_error_cnt                                       = 0;
        operate_parm.laser_operate_data.fault_code1.bit.t4 = 0;
    }
}

void GPIO_Check(void)
{
    /***************************************************
    以下为水压输入引脚状态检测种子源故障信号
    ****************************************************/

#if (DEVICE_ID == 3)
    if (HAL_GPIO_ReadPin(Water_Press_GPIO, Water_Press_Pin) == OFF) {
        brake_state                                                = 1;
        operate_parm.laser_operate_data.fault_code2.bit.seedSrcErr = 1;
        Laser_Health_Refresh();
    }
#endif
    /***************************************************
    以下为QBH输入引脚状态检测
    ****************************************************/
    /*if (1 == adaptor_parm.part.laser_module_adaptor_parm[0].check_feature_2.part.qbh)
    {
      if (HAL_GPIO_ReadPin(QBH_GPIO, QBH_Pin) != ON)
      {
        qbh_err_cnt++;
        if (30 <= qbh_err_cnt)
        {
          qbh_err_cnt = 0;
          operate_parm.laser_operate_data[0].fault_code3.bit.qbh_err = 1;
          Laser_Health_Refresh();
        }
      }
      else
      {
        qbh_err_cnt = 0;
      }
    }
    else
    {
      qbh_err_cnt = 0;
      operate_parm.laser_operate_data[0].fault_code3.bit.qbh_err = 0;
    }*/
    /********************************************
    刹车引脚常规性检测
    *********************************************/
    if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_1.part.brake) {
        if (HAL_GPIO_ReadPin(Break_In_GPIO, Break_In_Pin) == ON)  // 高电平刹车
        {
            brake_cnt++;
            if (2 <= brake_cnt) {
                brake_cnt   = 2;
                brake_state = 1;
                EXTI->IMR &= ~(1 << 2);  // 屏蔽line2上的中断
                EXTI->PR = (1 << 2);
                Laser_Health_Refresh();
            }
        } else {
            brake_cnt = 0;
        }
    } else {
        brake_cnt   = 0;
        brake_state = 0;
    }
}

void pd_success_check_thread(ULONG arg)
{
    unsigned check_cnt = 10;
    ULONG    dst_id    = 0;
    while (1) {
        tx_queue_receive(&queue_emit_success, &dst_id, TX_WAIT_FOREVER);
        check_cnt = 150;
        while (check_cnt--) {
            if (operate_parm.laser_operate_data.PD3_Value >= 5) {
                Laser_Open_Success_Reply(dst_id);
                break;
            }
            tx_thread_sleep(10);
        }
    }
}

void pd1_check_thread(ULONG arg)
{
    while (1) {
        if (0x01 == pd1_check_start) {
            if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_1.part.pd1) {
                if (operate_parm.laser_operate_data.PD1_Value < adaptor_parm.part.laser_module_adaptor_parm.pd1_low_limit)  // 低于阈值，触发计数
                {
                    pd1_underlow_count++;          // 连续异常计数
                    if (10 <= pd1_underlow_count)  // 连续10次低于阈值 10×10mS=100ms
                    {
                        pd1_underlow_count                                  = 10;  // 故障计数清零
                        operate_parm.laser_operate_data.fault_code1.bit.pd1 = 1;   // 故障处理
                        Laser_Health_Refresh();
                    }
                } else  // 高于阈值，清除计数
                {
                    pd1_underlow_count = 0;
                }
            } else {
                pd1_underlow_count                                  = 0;
                operate_parm.laser_operate_data.fault_code1.bit.pd1 = 0;
            }
        } else {
            pd1_underlow_count = 0;
        }

        tx_thread_sleep(10);
    }
}

static uint16_t pd3DelayCnt1 = 0;

void pd2_check_thread(ULONG arg)
{
    static uint16_t pd3DelayCnt = 0;
    while (1) {
        if ((0x01 == operate_parm.laser_operate_data.Laser_State) && (10 <= operate_parm.laser_operate_data.light_intensity)) {
            if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_1.part.pd2) {
                if (operate_parm.laser_operate_data.PD2_Value > adaptor_parm.part.laser_module_adaptor_parm.pd2_low_limit)  // 高于阈值，触发计数
                {
                    pd2_underlow_count++;          // 连续异常计数
                    if (10 <= pd2_underlow_count)  // 连续10次低于阈值 10×10mS=100ms
                    {
                        pd2_underlow_count                                  = 10;  // 故障计数清零
                        operate_parm.laser_operate_data.fault_code1.bit.pd2 = 1;   // 故障处理
                        Laser_Health_Refresh();
                    }
                } else  // 高于阈值，清除计数
                {
                    pd2_underlow_count = 0;
                }
            } else {
                pd2_underlow_count                                  = 0;
                operate_parm.laser_operate_data.fault_code1.bit.pd2 = 0;
            }
        } else {
            pd2_underlow_count = 0;
        }
        if ((0x01 == operate_parm.laser_operate_data.Laser_State) && (10 <= operate_parm.laser_operate_data.light_intensity)) {
            if (operate_parm.laser_operate_data.PD3_Value < adaptor_parm.part.laser_module_adaptor_parm.pd3_low_limit)  // 低于阈值，触发计数
            {
                pd3DelayCnt1++;
            }
            if (pd3DelayCnt < 100) {
                pd3DelayCnt++;
            } else {
                if (1 == adaptor_parm.part.laser_module_adaptor_parm.check_feature_2.part.pd3) {
                    if (operate_parm.laser_operate_data.PD3_Value < adaptor_parm.part.laser_module_adaptor_parm.pd3_low_limit)  // 低于阈值，触发计数
                    {
                        pd3_underlow_count++;          // 连续异常计数
                        if (10 <= pd3_underlow_count)  // 连续10次低于阈值 10×10mS=100ms
                        {
                            pd3_underlow_count                                  = 10;  // 故障计数清零
                            operate_parm.laser_operate_data.fault_code1.bit.pd3 = 1;   // 故障处理
                            Laser_Health_Refresh();
                        }
                    } else  // 高于阈值，清除计数
                    {
                        pd3_underlow_count = 0;
                    }
                } else {
                    pd3_underlow_count                                  = 0;
                    operate_parm.laser_operate_data.fault_code1.bit.pd3 = 0;
                }
            }
        } else {
            pd3DelayCnt        = 0;
            pd3DelayCnt1       = 0;
            pd3_underlow_count = 0;
        }
        tx_thread_sleep(10);
    }
}

void pump_pd_check_thread(ULONG arg)
{
    while (1) {
        tx_thread_sleep(10);
    }
}

int judge_fault(void)
{
    int ret = 0;

    ret |= (0 != operate_parm.laser_operate_data.fault_code1.all) || (0 != operate_parm.laser_operate_data.fault_code2.all) || (0 != operate_parm.laser_operate_data.fault_code3.all) ||
           (4 == operate_parm.laser_operate_data.Laser_State) || (4 == operate_parm.power_operate_parm.power_state) || (1 == brake_state) || (1 == master_comm_err) ||
           (operate_parm.power_operate_parm.modul1_State & 0x7D) || (operate_parm.power_operate_parm.modul2_State & 0x7D) || (operate_parm.power_operate_parm.modul3_State & 0x7D) ||
           (operate_parm.power_operate_parm.modul4_State & 0x7D) || (operate_parm.seedSrc_operate_data.seedState == 2);
    return ret;
}

/*故障处理，状态刷新*/
void Laser_Health_Refresh(void)
{
    operate_parm.laser_operate_data.fault_code1.bit.brake_state = brake_state;
    if (judge_fault()) {
#if 0
    if(0x04 != operate_parm.laser_operate_data[0].Laser_State)
      Uart_Send_Command(&Response_Wait_Close_Control);
#endif

        C_FAULT(ON);
        HG_CTRL(OFF);
        operate_parm.laser_operate_data.Laser_State = 0x04;    // 切换到故障状态
        operate_parm.laser_operate_data.standby &= ~(1 << 2);  // 切换到冷待机
        pd1_check_start = 0;
    } else {
        C_FAULT(OFF);
        err_dispose_finish = 0;
        if (0x4 == operate_parm.laser_operate_data.Laser_State)
            operate_parm.laser_operate_data.Laser_State = 0x00;
    }
}
